#include "sensor/sensor_sync.h"

namespace sensor {

SensorSync::SensorSync() {

}

void SensorSync::AddImuData(const ImuData& imu_data) {
    if(imu_data.timestamp < last_timestamp_imu_) {
        imu_queue_.clear();
    }
    last_timestamp_imu_ = imu_data.timestamp;
    imu_queue_.push_back(imu_data);
}

void SensorSync::AddWheelOdometryData(const WheelData& wheel_data) {
    if(wheel_data.timestamp < last_timestamp_odom_) {
        wheel_queue_.clear();
    }
    last_timestamp_odom_ = wheel_data.timestamp;
    wheel_queue_.push_back(wheel_data);
}

void SensorSync::AddFeatureData(const double& timestamp, const std::vector<FeaturePoint>& feature_data) {
    if(timestamp < last_timestamp_feature_) {
        feature_queue_.clear();
    }

    time_queue_.push_back(timestamp);
    feature_queue_.push_back(feature_data);

    last_timestamp_feature_ = timestamp;

    Sync();
}

bool SensorSync::Sync() {
    if (feature_queue_.empty() || imu_queue_.empty() || wheel_queue_.empty()) {
        return false;
    }
    double feature_timestamp = time_queue_.front();
    if (!feature_pushed_) {
        measures_.timestamp = feature_timestamp;
        measures_.feature_data = feature_queue_.front();
        feature_pushed_ = true;
    }

    if (last_timestamp_imu_ < feature_timestamp ||
        last_timestamp_odom_ < feature_timestamp) {
        return false;
    }     

    double imu_time = imu_queue_.front().timestamp;
    measures_.imu_data.clear();
    while ((!imu_queue_.empty()) && (imu_time < feature_timestamp)) {
        imu_time = imu_queue_.front().timestamp;
        if (imu_time > feature_timestamp) {
            if(imu_time - feature_timestamp < 0.008) {
                measures_.imu_data.push_back(imu_queue_.front());
                imu_queue_.pop_front();
            }
            break;
        }
        measures_.imu_data.push_back(imu_queue_.front());
        imu_queue_.pop_front();
    }

    double odom_time = wheel_queue_.front().timestamp;
    measures_.wheel_data.clear();
    while ((!wheel_queue_.empty()) && (odom_time < feature_timestamp)) {
        odom_time = wheel_queue_.front().timestamp;

        // LOG(INFO) << "odom_time =========== : " << odom_time << ", feature : " << feature_timestamp << std::endl;

        if (odom_time > feature_timestamp) {
            if(odom_time - feature_timestamp < 0.008) {
                measures_.wheel_data.push_back(wheel_queue_.front());
                wheel_queue_.pop_front();
            }
            break;
        }
        measures_.wheel_data.push_back(wheel_queue_.front());
        wheel_queue_.pop_front();
    }   

    // LOG(INFO) << "sync wheel timestamp =========== : " << measures_.wheel_data.back().timestamp << ", feature : " << feature_timestamp << std::endl;

    feature_queue_.pop_front();
    time_queue_.pop_front();
    feature_pushed_ = false;

    return true;
}

}